#!/usr/bin/env python3

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, TimerAction, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch.actions import ExecuteProcess
from launch_ros.actions import Node

def generate_launch_description():
    # 1. 声明参数
    declare_world_cmd = DeclareLaunchArgument(
        'world',
        default_value='/home/c234/cyt/src/zc4_bringup/launch/zc4.world',
        description='Absolute path to the world file to load in Gazebo'
    )

    declare_robot_urdf_cmd = DeclareLaunchArgument(
        'robot_urdf',
        default_value='/home/c234/cyt/src/test_urdf/urdf/test_urdf.urdf',
        description='Absolute path to the URDF file of the robot'
    )

    # 2. 启动 Gazebo（带GUI）
    gazebo_ros_share = get_package_share_directory('gazebo_ros')
    gazebo_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(gazebo_ros_share, 'launch', 'gazebo.launch.py')
        ),
        launch_arguments={'world': LaunchConfiguration('world'),'use_sim_time': 'true'}.items()
    )

    # 3. 延迟后 spawn 机器人
    spawn_robot = TimerAction(
        period=5.0,
        actions=[
            ExecuteProcess(
                cmd=[
                    'ros2', 'run', 'gazebo_ros', 'spawn_entity.py',
                    '-entity', 'zc4_robot',
                    '-file', LaunchConfiguration('robot_urdf'),
                    '-x', '0', '-y', '0', '-z', '0'
                ],
                output='screen'
            )
        ]
    )

    # 4. 启动 MoveIt demo（demo.launch.py）
    package_name = 'zc4_moveit_config'
    moveit_demo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(
                get_package_share_directory(package_name),
                'launch',
                'demo.launch.py'
            )
        ),
        launch_arguments={'use_sim_time': 'true'}.items()
    )

    # 启动 controller_manager 节点（外部 ros2_control_node）
    controller_manager_node = Node(
        package='controller_manager',
        executable='ros2_control_node',
        parameters=[
            # 参数文件和 robot_description，一起传入：
            os.path.join(get_package_share_directory("zc4_moveit_config"), "config", "ros2_controllers.yaml")
        ],
        output='screen',
        # arguments=["--ros-args", "--log-level", "debug"]
    )

    # 组装 LaunchDescription
    ld = LaunchDescription()

    ld.add_action(declare_world_cmd)
    ld.add_action(declare_robot_urdf_cmd)
    ld.add_action(gazebo_launch)

    ld.add_action(controller_manager_node)  # 外部启动 controller_manager
    
    ld.add_action(spawn_robot)
    ld.add_action(moveit_demo)

    return ld
